﻿// Decompiled with JetBrains decompiler
// Type: DnProcessSimulateCommands.ExportToRTR.CApExportToRTRRobotPrototype
// Assembly: DnProcessSimulateCommands, Version=2402.0.0.890, Culture=neutral, PublicKeyToken=null
// MVID: A885D881-AEDF-447E-852A-6F13102E089C
// Assembly location: C:\Program Files\Tecnomatix_2402\eMPower\DotNetCommands\DnProcessSimulateCommands.dll

using DnProcessSimulateCommands.ExportToRTR.YAML;
using EngineeringInternalExtension;
using EngineeringInternalExtension.ModelObjects;
using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.IO;
using System.Linq;
using Tecnomatix.Engineering;


namespace DnProcessSimulateCommands.ExportToRTR{

public class CApExportToRTRRobotPrototype : CApExportToRTRExporter
{
  private TxRobot m_robot;
  private HashSet<TxKinematicLink> m_links = new HashSet<TxKinematicLink>();
  private int m_jointIndex;
  private TxReportWriterEx m_logWriter;
  private string m_robotPrototypeName;
  private RobotPrototype m_rtrRobot;
  private List<ILink> m_rtrLinks = new List<ILink>();
  private List<IJoint> m_rtrJoints = new List<IJoint>();
  internal const string ROBOT_TYPES = "robot_types";

  internal Dictionary<string, ITxObject> ExportedLinksWith3DNameToObjectMap
  {
    get
    {
      Dictionary<string, ITxObject> ret = new Dictionary<string, ITxObject>();
      this.m_links.ToList<TxKinematicLink>().ForEach((Action<TxKinematicLink>) (link => ret.Add(link.Name, (ITxObject) link)));
      return ret;
    }
  }

  public CApExportToRTRRobotPrototype(TxRobot robot, string root, TxReportWriterEx logWriter)
    : base(root, "", ((TxBaseGeometryCreationObject) robot).Name)
  {
    this.m_logWriter = logWriter;
    this.m_robot = robot;
    this.m_robotPrototypeName = this.ComputeUniquePrototypeName((ITxComponent) robot);
    this.m_directory = Path.Combine("robot_types" + Path.AltDirectorySeparatorChar.ToString(), CApExportToRTRUtils.GetValidFileName(this.m_robotPrototypeName));
  }

  public RobotPrototype Export()
  {
    this.m_jointIndex = 0;
    Directory.CreateDirectory(Path.Combine(this.m_root, this.m_directory));
    CApExportToRTRRobotPrototype.ReplaceInvalidChars(ref this.m_robotPrototypeName);
    this.m_rtrRobot = new RobotPrototype()
    {
      Name = this.m_robotPrototypeName,
      Oem = this.m_robot.Controller.Name
    };
    CApExportToRTRYamlMgr.Instance.AddRobotPrototype(this.m_rtrRobot);
    using (TxPoseData currentPose = this.m_robot.CurrentPose)
    {
      ((IEnumerable<ITxObject>) this.m_robot.DrivingJoints).ToList<ITxObject>().ForEach((Action<ITxObject>) (x => (x as TxJoint).CurrentValue = 0.0));
      this.ExportBaseLink();
      this.ExportDrivigJointsWithLinks();
      this.m_robot.CurrentPose = currentPose;
    }
    return this.m_rtrRobot;
  }

  internal static void BlankSubDevices(TxRobot robot)
  {
    foreach (ITxObject allDescendant in (Collection<ITxObject>) robot.GetAllDescendants((ITxTypeFilter) new TxTypeFilter(typeof (ITxDevice))))
      (allDescendant as ITxDisplayableObject).Blank();
  }

  private void ExportDrivigJointsWithLinks()
  {
    foreach (ITxObject drivingJoint in (Collection<ITxObject>) this.m_robot.DrivingJoints)
    {
      TxJoint joint = drivingJoint as TxJoint;
      if (TxJoint.op_Inequality(joint, (ITxObject) null))
        this.WriteJointWithLink(joint);
    }
  }

  private void WriteJointWithLink(TxJoint joint)
  {
    this.CreateJoint(joint);
    this.CreateChildLink(joint);
  }

  private void ExportBaseLink()
  {
    TxKinematicLink baseLink = this.GetBaseLink();
    if (!TxKinematicLink.op_Inequality(baseLink, (ITxObject) null))
      return;
    this.CreateRobotLink(baseLink);
  }

  private TxKinematicLink GetBaseLink()
  {
    List<TxKinematicLink> robotLinks = new List<TxKinematicLink>();
    ((IEnumerable<ITxObject>) this.m_robot.DrivingJoints).ToList<ITxObject>().ForEach((Action<ITxObject>) (x => robotLinks.Add((x as TxJoint).ParentLink)));
    foreach (TxKinematicLink baseLink in robotLinks)
    {
      if (TxJoint.op_Equality(baseLink.ParentJoint, (ITxObject) null))
        return baseLink;
    }
    return (TxKinematicLink) null;
  }

  private ILink CreateChildLink(TxJoint joint)
  {
    return this.CreateRobotLink(joint.ChildLink, CApExportToRTRYamlMgr.Instance.JointsOffset[this.GetJointKey(joint)]);
  }

  private ILink CreateRobotLink(TxKinematicLink kinematicLink, TxTransformation jointFrame = null)
  {
    if (this.m_links.Contains(kinematicLink))
      return (ILink) null;
    bool flag = kinematicLink.Visibility == 2;
    if (flag)
      kinematicLink.Display();
    Link link = this.CreateLink((ITxObject) kinematicLink, jointFrame);
    this.m_links.Add(kinematicLink);
    CApExportToRTRYamlMgr.Instance.AddRobotKinematicChild<Link>(this.m_rtrRobot, link, (ITxObject) kinematicLink);
    if (flag)
      kinematicLink.Blank();
    return (ILink) link;
  }

  private Link CreateLink(ITxObject kinematicLink, TxTransformation jointFrame = null)
  {
    string name = kinematicLink.Name;
    string stlFileName = CApExportToRTRSTLWriter.GetStlFileName(kinematicLink, false, ".jt");
    TxTransformation transformation = TxTransformation.op_Inequality(jointFrame, (TxTransformation) null) ? jointFrame.Inverse : new TxTransformation();
    Link link = new Link()
    {
      Name = name,
      CadPath = Path.Combine(this.m_directory + "/", stlFileName),
      OffsetXyz = new Vec3D(transformation.Translation),
      OffsetRpy = new Vec3D(transformation.GetRotation())
    };
    this.CreateCadFile(kinematicLink, stlFileName);
    return link;
  }

  private IJoint CreateJoint(TxJoint joint)
  {
    TxVector jointFrom;
    TxVector jointTo;
    CApExportToRTRRobotPrototype.GetAxisPointsInDeviceCoordinates(joint, out jointFrom, out jointTo);
    TxTransformation txTransformation = CApExportToRTRRobotPrototype.CalcJointFrame(jointFrom, jointTo);
    CApExportToRTRYamlMgr.Instance.JointsOffset.Add(this.GetJointKey(joint), txTransformation);
    TxTransformation jointOrigin = this.GetJointOrigin(joint, joint.ParentLink.ParentJoint);
    Joint joint1 = new Joint()
    {
      Name = joint.Name,
      Parent = new ObjectRef()
      {
        Link = joint.ParentLink.Name
      },
      Child = new ObjectRef()
      {
        Link = joint.ChildLink.Name
      },
      Type = CApExportToRTRRobotPrototype.GetJointType(joint),
      OffsetXyz = new Vec3D(jointOrigin.Translation),
      OffsetRpy = new Vec3D(jointOrigin.GetRotation()),
      Limits = new JointLimits()
      {
        MinPosition = new double?(joint.LowerSoftLimit),
        MaxPosition = new double?(joint.UpperSoftLimit),
        MaxVelocity = new double?(joint.MaxSpeed),
        MaxAcceleration = new double?(joint.MaxAcceleration),
        MaxJerk = new double?(100000.0)
      },
      Axis = new Vec3D(this.CalcJointAxis(joint, jointFrom, jointTo)),
      DriverIndex = new int?(this.m_jointIndex++)
    };
    if (joint.HardLimitsData.HardLimitsType == 1)
    {
      joint1.Limits.MinPosition = new double?(joint.HardLimitsData.LowerHardLimit);
      joint1.Limits.MaxPosition = new double?(joint.HardLimitsData.UpperHardLimit);
    }
    this.HanldeCoupling(joint, joint1);
    CApExportToRTRYamlMgr.Instance.AddRobotKinematicChild<Joint>(this.m_rtrRobot, joint1, (ITxObject) joint);
    return (IJoint) joint1;
  }

  private void HanldeCoupling(TxJoint joint, Joint rtrJoint)
  {
    if (!TxJointEx.IsFollowing(joint))
      return;
    rtrJoint.Coupling = new Coupling()
    {
      LeadingJoint = TxJointEx.GetLeadingJoints(joint)[0].FollowJoint.Name,
      FollowFactor = TxJointEx.GetLeadingJoints(joint)[0].Factor
    };
    Joint robotKinematicChild = CApExportToRTRYamlMgr.Instance.GetRobotKinematicChild(this.m_rtrRobot, (ITxObject) TxJointEx.GetLeadingJoints(joint)[0].FollowJoint) as Joint;
    double? nullable1;
    if (joint.HardLimitsData.HardLimitsType == 2)
    {
      TxJointVariableLimitsData variableHardLimitsData = joint.HardLimitsData.VariableHardLimitsData;
      if (variableHardLimitsData != null)
      {
        TxJoint dependsOn = variableHardLimitsData.DependsOn;
        TxVariableLimitsData[] limits = variableHardLimitsData.Limits;
        VariablePoints variablePoints1 = new VariablePoints();
        VariablePoints variablePoints2 = new VariablePoints();
        foreach (TxVariableLimitsData variableLimitsData in limits)
        {
          variablePoints1.Points.Add(new DictionaryFlowStyle()
          {
            Data = {
              {
                dependsOn.Name,
                variableLimitsData.DependOnJointValue
              },
              {
                joint.Name,
                variableLimitsData.JointUpperLimitValue
              }
            }
          });
          variablePoints2.Points.Add(new DictionaryFlowStyle()
          {
            Data = {
              {
                dependsOn.Name,
                variableLimitsData.DependOnJointValue
              },
              {
                joint.Name,
                variableLimitsData.JointLowerLimitValue
              }
            }
          });
        }
        variablePoints2.Points.Reverse();
        variablePoints1.Points.AddRange((IEnumerable<DictionaryFlowStyle>) variablePoints2.Points);
        this.m_rtrRobot.VariableJointLimits.Add(variablePoints1);
      }
    }
    else
    {
      VariablePoints variableLimit = new VariablePoints();
      this.AddVariableLimitPoint(robotKinematicChild.Name, robotKinematicChild.Limits.MinPosition.Value, rtrJoint.Name, rtrJoint.Limits.MinPosition.Value, ref variableLimit);
      string name1 = robotKinematicChild.Name;
      double leadingValue1 = robotKinematicChild.Limits.MaxPosition.Value;
      string name2 = rtrJoint.Name;
      nullable1 = rtrJoint.Limits.MinPosition;
      double followingValue1 = nullable1.Value;
      ref VariablePoints local1 = ref variableLimit;
      this.AddVariableLimitPoint(name1, leadingValue1, name2, followingValue1, ref local1);
      string name3 = robotKinematicChild.Name;
      nullable1 = robotKinematicChild.Limits.MaxPosition;
      double leadingValue2 = nullable1.Value;
      string name4 = rtrJoint.Name;
      nullable1 = rtrJoint.Limits.MaxPosition;
      double followingValue2 = nullable1.Value;
      ref VariablePoints local2 = ref variableLimit;
      this.AddVariableLimitPoint(name3, leadingValue2, name4, followingValue2, ref local2);
      string name5 = robotKinematicChild.Name;
      nullable1 = robotKinematicChild.Limits.MinPosition;
      double leadingValue3 = nullable1.Value;
      string name6 = rtrJoint.Name;
      nullable1 = rtrJoint.Limits.MaxPosition;
      double followingValue3 = nullable1.Value;
      ref VariablePoints local3 = ref variableLimit;
      this.AddVariableLimitPoint(name5, leadingValue3, name6, followingValue3, ref local3);
      this.m_rtrRobot.VariableJointLimits.Add(variableLimit);
    }
    JointLimits limits1 = rtrJoint.Limits;
    JointLimits limits2 = rtrJoint.Limits;
    nullable1 = new double?();
    double? nullable2 = nullable1;
    limits2.MaxPosition = nullable2;
    double? nullable3 = nullable1;
    limits1.MinPosition = nullable3;
    JointLimits limits3 = robotKinematicChild.Limits;
    JointLimits limits4 = robotKinematicChild.Limits;
    nullable1 = new double?();
    double? nullable4 = nullable1;
    limits4.MaxPosition = nullable4;
    double? nullable5 = nullable1;
    limits3.MinPosition = nullable5;
  }

  private void AddVariableLimitPoint(
    string leadingJoint,
    double leadingValue,
    string followingJoint,
    double followingValue,
    ref VariablePoints variableLimit)
  {
    variableLimit.Points.Add(new DictionaryFlowStyle()
    {
      Data = {
        {
          leadingJoint,
          leadingValue
        },
        {
          followingJoint,
          followingValue
        }
      }
    });
  }

  private TxTransformation GetJointOrigin(TxJoint joint, TxJoint parentJoint)
  {
    TxTransformation jointOrigin = CApExportToRTRYamlMgr.Instance.JointsOffset[this.GetJointKey(joint)];
    if (TxJoint.op_Inequality(parentJoint, (ITxObject) null))
      jointOrigin = TxTransformation.op_Multiply(CApExportToRTRYamlMgr.Instance.JointsOffset[this.GetJointKey(parentJoint)].Inverse, jointOrigin);
    return jointOrigin;
  }

  internal static TxTransformation CalcJointFrame(TxVector jointFrom, TxVector jointTo)
  {
    return new TxTransformation()
    {
      Translation = CApExportToRTRRobotPrototype.CalcLocation(jointFrom, jointTo)
    };
  }

  private static TxVector CalcLocation(TxVector jointFrom, TxVector jointTo)
  {
    TxVector txVector = CApExportToRTRRobotPrototype.CalcJointDirection(jointFrom, jointTo);
    double num = Math.Sqrt(txVector.X * txVector.X + txVector.Y * txVector.Y + txVector.Z * txVector.Z) / 2.0;
    return TxVector.op_Addition(jointFrom, TxVector.op_Multiply(txVector, num));
  }

  internal static TxVector CalcJointDirection(TxVector jointFrom, TxVector jointTo)
  {
    TxVector txVector = TxVector.op_Subtraction(jointTo, jointFrom);
    txVector.Normalize();
    return txVector;
  }

  internal static void GetAxisPointsInDeviceCoordinates(
    TxJoint joint,
    out TxVector jointFrom,
    out TxVector jointTo)
  {
    TxTransformation absoluteLocation = (joint.Device as ITxLocatableObject).AbsoluteLocation;
    jointFrom = TxTransformation.op_Multiply(absoluteLocation.Inverse, joint.Axis.FromPoint);
    jointTo = TxTransformation.op_Multiply(absoluteLocation.Inverse, joint.Axis.ToPoint);
  }

  private TxVector CalcJointAxis(TxJoint joint, TxVector jointFrom, TxVector jointTo)
  {
    TxVector txVector = CApExportToRTRRobotPrototype.CalcJointDirection(jointFrom, jointTo);
    return CApExportToRTRYamlMgr.Instance.JointsOffset[this.GetJointKey(joint)].TransformNormal(txVector);
  }

  internal static string GetJointType(TxJoint joint) => joint.Type.ToString().ToLower();

  private string GetJointKey(TxJoint joint)
  {
    return CApExportToRTRYamlUtils.GetRobotKinematicChildKey(this.m_robotPrototypeName, (ITxObject) joint);
  }

  private string ComputeUniquePrototypeName(ITxComponent component)
  {
    string name = Path.GetFileNameWithoutExtension(TxComponentEx.GetComponentPath(component, false));
    if (Directory.Exists(Path.Combine(this.m_root, "robot_types", CApExportToRTRUtils.GetValidFileName(name))))
      name = $"{name}_{((ITxObject) component).Id.Replace(",", "_")}";
    return name;
  }

  private static void ReplaceInvalidChars(ref string name)
  {
    char[] array = name.Where<char>((Func<char, bool>) (c => (!char.IsLetterOrDigit(c) || c >= '\u0080') && c != '_')).ToArray<char>();
    name = string.Join("_", name.Split(array));
  }
}
